#include "mathfunctions.h"
#include <assert.h>
#include <math.h>

     
     
vector2_t pose2_add_pos(vector2_t a, pose2_t b) {
	double co, si;
	vector2_t c;
	co = cos(b.rot);
	si = sin(b.rot);
	c.x = co * a.x - si * a.y + b.pos.x;
	c.y = si * a.x + co * a.y + b.pos.y;
	return c;
}

/* Add two poses (rotation component). */
double pose2_add_rot(double a, pose2_t b) {
	double c;
	c = atan2(sin(a + b.rot), cos(a + b.rot));
	return c;
}


vector2_t pose2_sub_pos(vector2_t a, pose2_t b) {
	double co, si;
	vector2_t c;
	co = cos(b.rot);
	si = sin(b.rot);
	c.x = +co * (a.x - b.pos.x) + si * (a.y - b.pos.y);
	c.y = -si * (a.x - b.pos.x) + co * (a.y - b.pos.y);

	return c;
}

/* Subtract two poses (rotation component). */
double pose2_sub_rot(double a, pose2_t b) {
	double c;
	c = atan2(sin(a - b.rot), cos(a - b.rot));
	return c;
}


/* Add two poses. */
pose2_t pose2_add(pose2_t a, pose2_t b) {
	pose2_t c;
	c.pos = pose2_add_pos(a.pos, b);
	c.rot = pose2_add_rot(a.rot, b);
	return c;
}


/* Subtract poses: c = a - b. */
pose2_t pose2_sub(pose2_t a, pose2_t b) {
	pose2_t c;
	c.pos = pose2_sub_pos(a.pos, b);
	c.rot = pose2_sub_rot(a.rot, b);
	return c;
}





